import struct
from ctypes import create_string_buffer
from Util.util import checkACC

def Send510ContralCode(leftspeed, rightspeed, armState):
    TxDataBuffer = create_string_buffer(13)
    TxDataBuffer[0] = b'\xA5'
    if leftspeed < 0.0:
        LeftOrientation = 0x08
    elif leftspeed > 0.0:
        LeftOrientation = 0x04
    else:
        LeftOrientation = 0x00

    if rightspeed < 0.0:
        RightOrientation = 0x02
    elif rightspeed > 0.0:
        RightOrientation = 0x01
    else:
        RightOrientation = 0x00

    Orientation = LeftOrientation | RightOrientation

    TxDataBuffer[1] = struct.pack('B', Orientation)
    TxDataBuffer[2] = struct.pack('B', abs(leftspeed))   #左轮速度
    TxDataBuffer[3] = struct.pack('B', abs(rightspeed))  #右轮速度

    TxDataBuffer[4] = b'\x00'       #底盘旋转控制 & 大臂俯仰控制
    TxDataBuffer[5] = b'\x00'       #小臂俯仰控制 & 腕关节俯仰控制

    if armState == 0:
        TxDataBuffer[6] = b'\x00'  # 手爪旋转控制 & 前支臂控制
    elif armState == -1:
        TxDataBuffer[6] = b'\x0B'  # 手爪旋转控制 & 前支臂控制
    else:
        TxDataBuffer[6] = b'\x03'  # 手爪旋转控制 & 前支臂控制

    TxDataBuffer[7] = b'\x00'       #云台旋转控制 & 云台俯仰控制
    TxDataBuffer[8] = b'\x00'       #手爪抓持控制
    TxDataBuffer[9] = b'\x00'       #底座摄像头俯仰控制 & 云台遮阳照 & 摆臂模式控制 & 机械臂模式控制 & 收放线控制
    TxDataBuffer[10] = b'\x00'      #云台灯光控制 & 激光瞄准控制 & 前向灯光控制 & 小液晶屏按键反馈
    TxDataBuffer[11] = b'\x00'      #图像四分屏控制 & 云台图像焦距设置

    TxDataBuffer[12] = struct.pack('B', checkACC(TxDataBuffer[:12]))
    return TxDataBuffer